#include "I2C_Driver.h"
#include "logger_rte.h"
#include <cassert>

#define I2C_OK 1
extern "C" void core_debug(const char *format, ...);

extern void delay(uint32_t num);

static const uint8_t MASTER_ADDRESS = 0x01;

/********************************************************/

void I2C_Driver::start() {
#ifdef ARDUINO
//    pinMode(scl, OUTPUT);
//    pinMode(sda, OUTPUT_OPEN_DRAIN);
    pin_function(scl, STM_PIN_DATA(STM_MODE_OUTPUT_PP, GPIO_NOPULL, 0));
    pin_function(sda, STM_PIN_DATA(STM_MODE_OUTPUT_OD, GPIO_NOPULL, 0));
#else
    LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOB);
    LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_GPIOF);
    LL_GPIO_InitTypeDef GPIO_InitStruct;
    /* Configure I2C1 pins: SCL and SDA */
#ifdef KT1264
    GPIO_InitStruct.Pin = LL_GPIO_PIN_1;
    GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
    GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN;
    LL_GPIO_Init(GPIOF, &GPIO_InitStruct);
    GPIO_InitStruct.Pin = LL_GPIO_PIN_0;
    GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
    GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_HIGH;
    GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN;
    LL_GPIO_Init(GPIOF, &GPIO_InitStruct);
#else
    GPIO_InitStruct.Pin = LL_GPIO_PIN_8;
    GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
    GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN;
    LL_GPIO_Init(GPIOB, &GPIO_InitStruct);
    GPIO_InitStruct.Pin = LL_GPIO_PIN_9;
    GPIO_InitStruct.Mode = LL_GPIO_MODE_OUTPUT;
    GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN;
    LL_GPIO_Init(GPIOB, &GPIO_InitStruct);
#endif
#endif
}

void I2C_delay(void) {
    volatile int i;
    // 这里可以优化速度	，经测试最低到5还能写入
    for (i = 100; i > 0;) {
        i--;
    }
}

bool I2C_Driver::I2C_Start(void) {

    SDA_H();
    SCL_H();
    I2C_delay();

    if (!SDA_read())
        return 0; // SDA线为低电平则总线忙,退出

    SDA_L();
    I2C_delay();

    if (SDA_read())
        return 0; // SDA线为高电平则总线出错,退出

    SDA_L();
    I2C_delay();
    return 1;
}

void I2C_Driver::I2C_Stop(void) {
    SCL_L();
    I2C_delay();
    SDA_L();
    I2C_delay();
    SCL_H();
    I2C_delay();
    SDA_H();
    I2C_delay();
}

void I2C_Driver::I2C_Ack(void) {
    SCL_L();
    I2C_delay();
    SDA_L();
    I2C_delay();
    SCL_H();
    I2C_delay();
    SCL_L();
    I2C_delay();
}

void I2C_Driver::I2C_NoAck(void) {
    SCL_L();
    I2C_delay();
    SDA_H();
    I2C_delay();
    SCL_H();
    I2C_delay();
    SCL_L();
    I2C_delay();
}

bool I2C_Driver::I2C_WaitAck(void)
// 返回为:=1有ACK,=0无ACK
{
    SCL_L();
    I2C_delay();
    SDA_H();
    I2C_delay();
    SCL_H();
    I2C_delay();
    if (SDA_read()) {
        SCL_L();
        return 0;
    }
    SCL_L();
    return 1;
}

int I2C_Driver::I2C_SendByte(u8 SendByte)
// 数据从高位到低位//
{
    u8 i = 8;
    while (i--) {
        SCL_L();
        I2C_delay();
        if (SendByte & 0x80)
            SDA_H();
        else
            SDA_L();
        SendByte <<= 1;
        I2C_delay();
        SCL_H();
        I2C_delay();
    }
    SCL_L();
    return 1;
}

u8 I2C_Driver::I2C_ReceiveByte(void)
// 数据从高位到低位//
{
    u8 i = 8;
    u8 ReceiveByte = 0;

    SDA_H();

    while (i--) {
        ReceiveByte <<= 1;
        SCL_L();
        I2C_delay();
        SCL_H();
        I2C_delay();
        if (SDA_read()) {
            ReceiveByte |= 0x01;
        }
    }
    SCL_L();

    return ReceiveByte;
}

// 写入1字节数据       待写入数据    待写入地址       器件类型(24c16或SD2403)
bool I2C_Driver::I2C_WriteByte(u8 SendByte, u8 WriteAddress) {
    if (!I2C_Start())
        return 0;
    I2C_SendByte(WriteAddress);
    // 设置高起始地址+器件地址
    if (!I2C_WaitAck()) {
        I2C_Stop();
        return 0;
    }
    // write((u8)(WriteAddress & 0x00FF)); //设置低起始地址
    // I2C_WaitAck();
    I2C_SendByte(SendByte);
    I2C_WaitAck();
    I2C_Stop();
    return 1;
}

// 注意不能跨页写
// 写入1串数据      待写入数组地址    待写入长度      待写入地址       器件类型(24c16或SD2403)
bool I2C_Driver::I2C_BufferWrite(u8 *pBuffer, u8 length, u8 WriteAddress) {
    if (!I2C_Start())
        return 0;
    I2C_SendByte(WriteAddress);
    // 设置高起始地址+器件地址
    if (!I2C_WaitAck()) {
        I2C_Stop();
        return 0;
    }
    // I2C_SendByte((u8) (WriteAddress));
    // 设置低起始地址
    // I2C_WaitAck();

    while (length--) {
        I2C_SendByte(*pBuffer);
        I2C_WaitAck();
        pBuffer++;
    }
    I2C_Stop();
    return 1;
}

#define I2C_PageSize 8

// 跨页写入1串数据  待写入数组地址    待写入长度      待写入地址       器件类型(24c16或SD2403)
void I2C_Driver::I2C_PageWrite(u8 *pBuffer, u8 length, u16 WriteAddress, u8 DeviceAddress) {
    u8 NumOfPage = 0, NumOfSingle = 0, Addr = 0, count = 0;
    Addr = WriteAddress % I2C_PageSize;
    // 写入地址是开始页的第几位
    count = I2C_PageSize - Addr;
    // 在开始页要写入的个数
    NumOfPage = length / I2C_PageSize;
    // 要写入的页数
    NumOfSingle = length % I2C_PageSize;
    // 不足一页的个数

    if (Addr == 0)
        // 写入地址是页的开始
    {
        if (NumOfPage == 0)
            // 数据小于一页
        {
            I2C_BufferWrite(pBuffer, NumOfSingle, WriteAddress);
            // 写少于一页的数据
        } else
            // 数据大于等于一页
        {
            while (NumOfPage)
                // 要写入的页数
            {
                I2C_BufferWrite(pBuffer, I2C_PageSize, WriteAddress);
                // 写一页的数据
                WriteAddress += I2C_PageSize;
                pBuffer += I2C_PageSize;
                NumOfPage--;
                delay(10);
            }
            if (NumOfSingle != 0)
                // 剩余数据小于一页
            {
                I2C_BufferWrite(pBuffer, NumOfSingle, WriteAddress);
                // 写少于一页的数据
                delay(10);
            }
        }
    } else
        // 写入地址不是页的开始
    {
        if (NumOfPage == 0)
            // 数据小于一页
        {
            I2C_BufferWrite(pBuffer, NumOfSingle, WriteAddress);
            // 写少于一页的数据
        } else
            // 数据大于等于一页
        {
            length -= count;
            NumOfPage = length / I2C_PageSize;
            // 重新计算要写入的页数
            NumOfSingle = length % I2C_PageSize;
            // 重新计算不足一页的个数

            if (count != 0) {
                I2C_BufferWrite(pBuffer, count, WriteAddress);
                // 将开始的空间写满一页
                WriteAddress += count;
                pBuffer += count;
            }

            while (NumOfPage--)
                // 要写入的页数
            {
                I2C_BufferWrite(pBuffer, I2C_PageSize, WriteAddress);
                // 写一页的数据
                WriteAddress += I2C_PageSize;
                pBuffer += I2C_PageSize;
            }
            if (NumOfSingle != 0)
                // 剩余数据小于一页
            {
                I2C_BufferWrite(pBuffer, NumOfSingle, WriteAddress);
                // 写少于一页的数据
            }
        }
    }
}

// 读出1串数据         存放读出数据  待读出长度      待读出地址       器件类型(24c16或SD2403)
bool I2C_Driver::I2C_ReadByte(u8 *pBuffer, u8 length, u8 ReadAddress) {
    if (!I2C_Start())
        return 0;
    I2C_SendByte(ReadAddress);
    // 设置高起始地址+器件地址
    if (!I2C_WaitAck()) {
        I2C_Stop();
        return 0;
    }
    I2C_SendByte((u8) (0));
    //设置低起始地址
    I2C_WaitAck();
    I2C_Start();
    I2C_SendByte(ReadAddress | 0x0001);
    I2C_WaitAck();
    while (length) {
        *pBuffer = I2C_ReceiveByte();
        if (length == 1)
            I2C_NoAck();
        else
            I2C_Ack();
        pBuffer++;
        length--;
    }
    I2C_Stop();
    return 1;
}

const GPIO_TypeDef *ports[] = {GPIOA, GPIOB, GPIOC, GPIOD, GPIOE, GPIOF};

void I2C_Driver::SDA_H() {
#ifdef ARDUINO
    digitalWriteFast(sda, HIGH);
#else
    LL_GPIO_SetOutputPin((GPIO_TypeDef *)ports[sda.port], sda.pin);
#endif
}

void I2C_Driver::SDA_L() {
#ifdef ARDUINO
    digitalWriteFast(sda, LOW);
#else
    LL_GPIO_ResetOutputPin((GPIO_TypeDef *)ports[sda.port], sda.pin);
#endif
}

void I2C_Driver::SCL_L() {
#ifdef ARDUINO
    digitalWriteFast(scl, LOW);
#else
    LL_GPIO_ResetOutputPin((GPIO_TypeDef *)ports[scl.port], scl.pin);
#endif
}

void I2C_Driver::SCL_H() {
#ifdef ARDUINO
    digitalWriteFast(scl, HIGH);
#else
    LL_GPIO_SetOutputPin((GPIO_TypeDef *)ports[scl.port], scl.pin);
#endif
}

int I2C_Driver::SDA_read() {
#ifdef ARDUINO
    int r = digitalReadFast(sda);
#else
    // pinMode(sda, INPUT_PULLUP);
    int r = LL_GPIO_IsInputPinSet((GPIO_TypeDef *)ports[sda.port], sda.pin) ? 1 : 0;
    // pinMode(sda, OUTPUT);
#endif
    return r;
}

int I2C_Driver::write(uint8_t data) {
    int ret = 1;
    if (transmitting) {
        // in master transmitter mode
        allocateTxBuffer(txBufferLength + 1);
        // error if no memory block available to allocate the buffer
        if (txBuffer == nullptr) {
//            setWriteError();
            ret = 0;
        } else {
            // put byte in tx buffer
            txBuffer[txBufferIndex] = data;
            ++txBufferIndex;
            // update amount in buffer
            txBufferLength = txBufferIndex;
        }
    } else {
        // in slave send mode
        // reply to master
        if (I2C_SendByte(data) != 0) {
            ret = 0;
        }
    }
    return ret;
}

/**
 * @brief  This function must be called in slave Tx event callback or after
 *         beginTransmission() and before endTransmission().
 * @param  pdata: pointer to the buffer data
 * @param  quantity: number of bytes to write
 * @retval number of bytes ready to write.
 */
int I2C_Driver::write(const uint8_t *data, int quantity) {
    int ret = quantity;

    if (transmitting) {
        // in master transmitter mode
        allocateTxBuffer(txBufferLength + quantity);
        // error if no memory block available to allocate the buffer
        if (txBuffer == nullptr) {
//            setWriteError();
            ret = 0;
        } else {
            // put bytes in tx buffer
            memcpy(&(txBuffer[txBufferIndex]), data, quantity);
            txBufferIndex = txBufferIndex + quantity;
            // update amount in buffer
            txBufferLength = txBufferIndex;
        }
    } else {
        // in slave send mode
        // reply to master
        if (I2C_BufferWrite((uint8_t *) data, quantity, txAddress) != 0) {
            ret = 0;
        }
    }
    return ret;
}

void I2C_Driver::beginTransmission(uint8_t adr) {
    txAddress = adr << 1;
    transmitting = 1;
}

uint8_t I2C_Driver::endTransmission(uint8_t sendStop) {
    int8_t ret = -1;
    // reset Tx buffer
    if (I2C_BufferWrite(txBuffer, txBufferLength, txAddress)) {
        ret = 0;
    }
    resetTxBuffer();

    // reset tx buffer iterator vars
    txBufferIndex = 0;
    txBufferLength = 0;

    // indicate that we are done transmitting
    transmitting = 0;

    return ret;
}

void I2C_Driver::allocateTxBuffer(int length) {
    if (txBufferAllocated < length) {
        // By default, we allocate BUFFER_LENGTH bytes. It is the min size of the buffer.
        if (length < BUFFER_LENGTH) {
            length = BUFFER_LENGTH;
        }
        uint8_t *tmp = (uint8_t *) realloc(txBuffer, length * sizeof(uint8_t));
        if (tmp != nullptr) {
            txBuffer = tmp;
            txBufferAllocated = length;
        } else {
            core_debug("No enough memory! (%i)\n", length);
        }
    }
}

void I2C_Driver::begin(uint8_t address, bool generalCall) {
    rxBufferIndex = 0;
    rxBufferLength = 0;
    rxBuffer = nullptr;
    rxBufferAllocated = 0;
    resetRxBuffer();

    txBufferIndex = 0;
    txBufferLength = 0;
    txAddress = 0;
    txBuffer = nullptr;
    txBufferAllocated = 0;
    resetTxBuffer();
    start();
}

uint8_t I2C_Driver::requestFrom(uint8_t address, uint8_t quantity, uint32_t iaddress, uint8_t isize, uint8_t sendStop) {
#if !defined(I2C_OTHER_FRAME)
    UNUSED(sendStop);
#endif
    uint8_t read = 0;

    allocateRxBuffer(quantity);
    // error if no memory block available to allocate the buffer
    if (rxBuffer == nullptr) {
//        setWriteError();
    } else {

        if (isize > 0) {
            // send internal address; this mode allows sending a repeated start to access
            // some devices' internal registers. This function is executed by the hardware
            // TWI module on other processors (for example Due's TWI_IADR and TWI_MMR registers)

            beginTransmission(address);

            // the maximum size of internal address is 3 bytes
            if (isize > 3) {
                isize = 3;
            }

            // write internal register address - most significant byte first
            while (isize-- > 0) {
                write((uint8_t) (iaddress >> (isize * 8)));
            }
            endTransmission(false);
        }

        if (I2C_OK == I2C_ReadByte(rxBuffer, quantity, address << 1)) {
            read = quantity;
        }

        // set rx buffer iterator vars
        rxBufferIndex = 0;
        rxBufferLength = read;
    }

    return read;
}

int I2C_Driver::read() {
    int value = -1;

    // get each successive byte on each call
    if (rxBufferIndex < rxBufferLength) {
        value = rxBuffer[rxBufferIndex];
        ++rxBufferIndex;

        /* Commented as not I think it is not useful
         * but kept to show that it is possible to
         * reset rx buffer when no more data available */
        /*if(rxBufferIndex == rxBufferLength) {
          resetRxBuffer();
        }*/
    }
    return value;
}

int I2C_Driver::available() {
    return rxBufferLength - rxBufferIndex;
}

int I2C_Driver::peek() {
    int value = -1;

    if (rxBufferIndex < rxBufferLength) {
        value = rxBuffer[rxBufferIndex];
    }
    return value;
}

void I2C_Driver::flush() {
    rxBufferIndex = 0;
    rxBufferLength = 0;
    resetRxBuffer();
    txDataSize = 0;
    resetTxBuffer();
}

void I2C_Driver::begin(bool generalCall) {
    begin(MASTER_ADDRESS, generalCall = false);
}

void I2C_Driver::allocateRxBuffer(int length) {
    if (rxBufferAllocated < length) {
        // By default we allocate BUFFER_LENGTH bytes. It is the min size of the buffer.
        if (length < BUFFER_LENGTH) {
            length = BUFFER_LENGTH;
        }
        uint8_t *tmp = (uint8_t *) realloc(rxBuffer, length * sizeof(uint8_t));
        if (tmp != nullptr) {
            rxBuffer = tmp;
            rxBufferAllocated = length;
        } else {
            core_debug("No enough memory! (%i)\n", length);
        }
    }
}

int I2C_Driver::available_wait(int Delay) {
    return 0;
}

int I2C_Driver::read_wait(int timeout) {
    return 0;
}
